#include "ActionController.h"
#define MAX_BALLOON 30

void analizeDegree(ArActionDesired *myDesired, double degree,
		double degreeRange);
int actionSee(IplImage *dest, Balloon *targets);
bool thereAreTarget(int num_target);
void getRange(ArRangeDevice *mySonar, double *centralRange, double *leftRange,
		double *rightRange, double radius);

/*
 This is the ActionTurn constructor, note the use of constructor chaining
 with the ArAction. also note how it uses setNextArgument, which makes
 it so that other things can see what parameters this action has, and
 set them.  It also initializes the classes variables.
 */
ActionController::ActionController(double stopDistance, double controlAngle,
		double velocityOnTarget, double velocityNearHurdle, double maxVelocity,
		ActionGo *go, ActionTurn *turn, ActionScouting *scout,
		ActionTurn *turnObstacle, ArActionGoto *goToGoal) :
	ArAction("Controller") {

	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	myVelocityOnTarget = velocityOnTarget;
	myVelocityNearHurdle = velocityNearHurdle;
	myMaxVelocity = maxVelocity;

	myGo = go;
	myTurn = turn;
	myScout = scout;
	myGoToGoal = goToGoal;
	myTurnObstacle = turnObstacle;
	goOnTarget = false;

	connectToCam(1);
}

/*
 Sets the myRobot pointer (all setRobot overloaded functions must do this),
 finds the sonar device from the robot, and if the sonar isn't there,
 then it deactivates itself.
 */
void ActionController::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

/*
 This is the guts of the Turn action.
 */
ArActionDesired *ActionController::fire(ArActionDesired currentDesired) {

	/* Dichiarazione delle variabili  Visione*/
	IplImage *src = 0;
	int num_target = 0;
	Balloon targets[MAX_BALLOON];

	/* Dichiarazione delle variabili  Robot*/
	double centralRange, leftRange, rightRange;
	double radius = myRobot->getRobotRadius();
	bool target = false;
	double distCenter = 500, distLeft = 300, distRigh = 300;
	double degree = 0, degreeRange = 50;
	int near = 0;
	bool isY;

	double delta = 0;

	/* Acquisizione delle grandezze di interesse */
	myDesired.reset();

	/* Faccio un istantanea della scena */
	num_target=actionSee(src,targets);

	/* Leggo i valori dei Sonar */
	getRange(mySonar, &centralRange, &leftRange, &rightRange, radius);

	printf("centrale =%f sinistra=%f destra=%f \n", centralRange, leftRange,
			rightRange);

	target = thereAreTarget(num_target);

	/*Individuazione stato in cui andare
	 * State=1 scoppia pallone
	 * State=2 evita ostacolo
	 * State=3 raggiungi taget
	 * State=4 esplora
	 * Quando vado nello stato 2, dobbiamo controllare i sensori in modo opportuno
	 * per capire se devo aggirare o meno un ostacolo per raggiungere il pallone giallo
	 * */

	isY=isYellow(src,cvPoint(src->width/2,src->height),144,128,40);
	// se ho qualcosa d'avanti ed è giallo o sto seguendo un target sto nello stato scoppia palloncino
	if ((centralRange < distCenter && isY == true) || goOnTarget == true)
		state = SCOPPIA_PALLONE;
	// se non ho qualcosa d'avanti ed è giallo o non sto seguendo un target sto negli altri stati
	else {
		// se ho qualcosa vicino (avanti, sinistra o destra) evito l'ostacolo
		if (centralRange < distCenter || leftRange < distLeft || rightRange
				< distRigh) {
			state = EVITA_OSTACOLO;
			// se non ho nulla vicino
		} else {
			//se ho un obbiettivo lo raggiungo
			if (target)
				state = RAGGIUNGI_TARGET;
			//altrimenti vado in esplorazione
			else
				state = ESPLORA;
		}
	}

	/*Esecuzione delle azioni opportune*/

	switch (state) {
	case SCOPPIA_PALLONE:
		printf("State1: Scoppio Pallone \n");
		//disattivo azioni
		if (myRobot->findAction("Turn") != NULL)
			myRobot->remAction(myTurn);
		if (myRobot->findAction("Go") != NULL)
			myRobot->remAction(myGo);
		if (myRobot->findAction("Scouting") != NULL)
			myRobot->remAction(myScout);
		//se ho il giallo avanti
		if (isY) {
			//imposto la posizione globale a 0,0
			myRobot->resetTripOdometer();
			//dichiaro il goal a distanza centralRange davanti al robot
			ArPose goal(centralRange, 0);
			myGoToGoal->setGoal(goal);
			goOnTarget = true;
		}
		//attivo le azioni goToGoal e turnObstacle
		if (myRobot->findAction("goto") == NULL)
			myRobot->addAction(myGoToGoal, 50);
		if (myRobot->findAction("avoidObstacles") == NULL)
			myRobot->addAction(myTurnObstacle, 60);

		//se ho raggiunto il goal resetto la variabile goOnTarget = ho scoppiato il pallone
		if (myGoToGoal->haveAchievedGoal() == true)
			goOnTarget = false;

		/*myDesired.setVel(myVelocityOnTarget);*/
		break;
	case EVITA_OSTACOLO:
		printf("State2: Evita Ostacolo \n");
		if (myRobot->findAction("Go") != NULL)
			myRobot->addAction(myGo, 50);

		if (myRobot->findAction("Scouting") != NULL)
			myRobot->remAction(myScout);

		if (myRobot->findAction("avoidObstacles") != NULL)
			myRobot->remAction(myTurnObstacle);

		if (myRobot->findAction("goto") != NULL)
			myRobot->remAction(myGoToGoal);

		if (myRobot->findAction("Turn") == NULL)
			myRobot->addAction(myTurn, 60);

		break;
	case RAGGIUNGI_TARGET:
		printf("State3: Raggiungi Target \n");

		myGo->myMaxSpeed = myMaxVelocity;

		near = nearest(targets, num_target);
		degree = targets[near].angle;

		if (myRobot->findAction("avoidObstacles") != NULL)
			myRobot->remAction(myTurnObstacle);

		if (myRobot->findAction("goto") != NULL)
			myRobot->remAction(myGoToGoal);

		if (myRobot->findAction("Turn") != NULL)
			myRobot->remAction(myTurn);

		if (myRobot->findAction("Scouting") != NULL)
			myRobot->remAction(myScout);

		if (myRobot->findAction("Go") == NULL)
			myRobot->addAction(myGo, 50);

		analizeDegree(&myDesired, degree, degreeRange);

		break;
	case ESPLORA:
		printf("State4: Esplora \n");

		if (myRobot->findAction("avoidObstacles") != NULL)
			myRobot->remAction(myTurnObstacle);

		if (myRobot->findAction("goto") != NULL)
			myRobot->remAction(myGoToGoal);

		if (myRobot->findAction("Turn") != NULL)
			myRobot->remAction(myTurn);

		if (myRobot->findAction("Go") != NULL)
			myRobot->addAction(myGo, 50);

		if (myRobot->findAction("Scout") == NULL)
			myRobot->addAction(myScout, 50);

		break;
	case EVITA_MAGNETIZZATA:
		printf("State5: Evita Magentizzata \n");
		break;
	}

	return &myDesired;
}

/* Questa azione acquisice un frame dalla scena e restituisce l'immagine e il numero di
 * target presenti
 * */
int actionSee(IplImage *dest, Balloon *targets) {
	dest = connectToCam(0);
	return getTargets(dest, targets, cvPoint(dest->width / 2, dest->height));
}
bool thereAreTarget(int num_target) {
	if (num_target > 0)
		return true;
	else
		return false;
}
void getRange(ArRangeDevice *mySonar, double *centralRange, double *leftRange,
		double *rightRange, double radius) {
	double centralAngle = 10;
	double leftAngle1 = 30, leftAngle2 = 50;
	*centralRange = mySonar->currentReadingPolar(-centralAngle, centralAngle)
			- radius;
	*leftRange = mySonar->currentReadingPolar(leftAngle1, leftAngle2) - radius;
	*rightRange = mySonar->currentReadingPolar(-leftAngle2, -leftAngle1)
			- radius;

}
void analizeDegree(ArActionDesired *myDesired, double degree,
		double degreeRange) {

	if (degree > degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(-10);
	} else if (degree < -degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(10);
	} else
		myDesired->setDeltaHeading(0);

}
